#ifndef __MOBILE__
#define __MOBILE__

#include "low-level\Triple.h"
#include "low-level\Matrix.h"
#include "low-level\Quaternion.h"

class __declspec( align( 16 ) ) Mobile
{
private:
	Triple pos; // position
	Triple P; // linear impulse
	Quaternion q; // rotation
	Triple L; // angular impulse

	Triple v; // linear speed
	Triple omega; // angular speed

	/* Constant quantities */
	Matrix *Ibody; /* inertia tensor */
	Matrix *Ibodyinv;

	/* Derived quantities (auxiliary variables) */
	Matrix Iinv;
	Matrix R;
	Matrix Rt; /*auxiliary variable */

	/* Computed quantities */
	Triple force; // linear force
	Triple torque; // notation found in Berkeley Physics Course Volume 1: M

	/* local */
	Triple loc__force; // local defined force (object space)
	Triple loc__torque; // local defined torque

	float m; // mass

	void *base_ptr;
public:
	Mobile();
	virtual ~Mobile();
	void* operator new(size_t);
    void TestMemoryLayout();
#ifdef USING_WORLD_BUFFERS
	void CreateWorldIndex();
#endif

	void SetPosition(float x, float y, float z) { this->pos[X] = x; this->pos[Y] = y; this->pos[Z] = z; }
	void SetRotation(float s, float vx, float vy, float vz) { this->q[S] = s; this->q[VX] = vx; this->q[VY] = vy; this->q[VZ] = vz; }
	FP_DATATYPE GetPosition(int index) { return this->pos[index]; }
	FP_DATATYPE GetRotation(int index) { return this->q[index]; };
	void SetLinearImpulse(float Px, float Py, float Pz) { this->P[X] = Px; this->P[Y] = Py; this->P[Z] = Pz; }
	void SetAngularImpulse(float Lx, float Ly, float Lz) { this->L[X] = Lx; this->L[Y] = Ly; this->L[Z] = Lz; }
	void SetMass(float value) { this->m = value; }
	void SetDefaultInertia();

	/* f is defined in WCS, r is defined in WCS and is the vector from mobile position to force action point */
	void AddForce(Triple& f, Triple& r);

	/* f is defined in OCS, r is defined in OCS and is the vector from mobile position to force action point */
	void AddLocalForce(Triple& loc__f, Triple& loc__r);

	virtual void AddGravitationalForce();

	void SetTorque(float x, float y, float z) { this->torque.Set(x, y, z); };

	void ResetForce();
	void UpdateForce();
	virtual void AddForce() { this->AddGravitationalForce(); };
	virtual void AddLocalForce() { /*  */ };

	void OCS_to_WCS(); // convert local (OCS) force and torque to world (WCS) force and torque

	void Translate(const FP_DATATYPE dt);
	void Rotate(const FP_DATATYPE dt);
	void Move(const FP_DATATYPE dt);

	void Move__rot_test(const FP_DATATYPE dt);
};

//void glMultMatrix(triple pos, matrix R); // multiply the current matrix with the specified matrix

#endif

/*
Mobile movement (translation&rotation) algorithm:

# update Rt (Rt = R translated) // TODO Why here?
# [abstract] update force
## force = 0
## add any local force
## add any global force
## convert any local force to global force (OCS_to_WCS)
# [abstract] translate
## update linear impulse -- P
## update linear speed, position
# [abstract] rotate
## update angular impulse -- L
## update rotation quaternion -- q (compute angular speed from L -- omega -- then dq...)
# normalize rotation quaternion
# update rotation matrix (R = q)

*/